#include <boost/thread/thread.hpp>
#include <localization/localization.h>

namespace {
void PrintLocalizationInfo(const player_pose2d *mean, const gsl_matrix *covariance);
}

LocalizationEstimator::LocalizationEstimator(const player_pose2d *initial_mean,
        const gsl_matrix *initial_covariance,
        const std::vector<player_point_2d> &mapped_lamps,
        PlayerCc::PlayerClient *player_client,
        PlayerCc::Position2dProxy *position2d_proxy,
        SyncedCurrentPose *current_pose, SyncedCurrentCovariance *current_covariance,
        SyncedLampCentroids *lamp_centroids) :
        current_pose_(current_pose), current_covariance_(current_covariance),
                dynamics_model_(position2d_proxy),
                vision_model_(mapped_lamps, lamp_centroids),
                player_client_(player_client) {

  mean_ = new player_pose2d;
  *mean_ = *initial_mean;

  covariance_ = gsl_matrix_alloc(3, 3);
  gsl_matrix_memcpy(covariance_, initial_covariance);
}

LocalizationEstimator::~LocalizationEstimator() {
  delete mean_;
  gsl_matrix_free(covariance_);
}

void LocalizationEstimator::operator()() {
  while (true) {
    player_client_->Read();

    dynamics_model_.UpdateBelief(mean_, covariance_);
    vision_model_.UpdateBelief(mean_, covariance_);

    current_pose_->set_current_pose(*mean_);
    current_covariance_->set_current_covariance(covariance_);

    if (kDebugMode)
      PrintLocalizationInfo(mean_, covariance_);

    boost::this_thread::sleep(
            boost::posix_time::milliseconds(kSleepTimeInMilliseconds));
  }
}

namespace Localization {
double AdjustAngle(double a) {
  while (a > kPi)
    a -= 2 * kPi;
  while (a < -kPi)
    a += 2 * kPi;
  return a;
}
}

namespace {
void PrintLocalizationInfo(const player_pose2d *mean, const gsl_matrix *covariance) {
  printf("Position Estimate: x = %lf, y = %lf, yaw = %lf\n", mean->px,
          mean->py, mean->pa * 180 / Localization::kPi);

  printf("Covariance Matrix:\n");
  for (int i = 0; i < 3; i++) {
    printf("[ ");
    printf("%8.5lf ", gsl_matrix_get(covariance, i, 0));
    printf("%8.5lf ", gsl_matrix_get(covariance, i, 1));
    printf("%8.5lf", gsl_matrix_get(covariance, i, 2));
    printf("]\n");
  }
  printf("\n");
}
}
